#include "motor.h"

extern TIM_HandleTypeDef htim8,htim2,htim3;

MotorInfo_TypeDef motor;
int16_t ros_encoder_left = 0;
int16_t ros_encoder_right = 0;


void Motor_Output(int32_t output_right,int32_t output_left)
{
	if( output_left > 0 )
	{
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,output_left);
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,0);
	}
	else if( output_left < 0 )
	{
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,0);
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,-output_left);	
	}
	else
	{
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,0);
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_2,0);			
	}
	//right
	
	
	if( output_right > 0 )
	{
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_3,output_right);
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_4,0);
	}
	else if( output_right < 0 ) 
	{
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_3,0);
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_4,-output_right);	
	}
	else
	{
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_3,0);
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_4,0);			
	}	
}

void Motor_UpdateEncoder(void)
{
	int16_t right = 0, left = 0;
	
	left = __HAL_TIM_GET_COUNTER(&htim2);
	right = __HAL_TIM_GET_COUNTER(&htim3);
	
	motor.left.encoder_cnt += left;
	motor.right.encoder_cnt += right;
	
	__HAL_TIM_SET_COUNTER(&htim2,0);
	__HAL_TIM_SET_COUNTER(&htim3,0);
}







